copp\copp\copp3/
interpolation.rs

1//! Interpolation and profile-conversion utilities for third-order path parameterization.
2//!
3//! # Method identity
4//! This module serves both:
5//! - **Time-Optimal Path Parameterization (TOPP3)** workflows,
6//! - **Convex-Objective Path Parameterization (COPP3)** workflows.
7//!
8//! # Scope
9//! This module provides deterministic conversions between:
10//! - node profiles `a(s) = \dot{s}^2` and `b(s) = \ddot{s}` sampled on stations,
11//! - time mapping `t(s)`,
12//! - inverse sampling `s(t)`.
13//!
14//! # Conventions
15//! - Path grid uses station samples `s[0..=n]`.
16//! - Both `a` and `b` are node-based in TOPP3/COPP3 (`a.len() == b.len() == s.len()`).
17//! - `num_stationary = (head, tail)` indicates stationary boundary counts at start/end.
18
19use crate::copp::InterpolationMode;
20use crate::math::numerical::{EPS_ZERO, solve_2x2};
21use itertools::izip;
22
23/// Compute cumulative time profile `t(s)` from TOPP3/COPP3 profiles `a(s), b(s)`.
24///
25/// # Semantics
26/// - `t_s[i]` is the time at station `s[i]`.
27/// - initial condition is `t_s[0] = t0`.
28/// - returns `(t_final, t_s)` where `t_final == *t_s.last().unwrap()`.
29///
30/// # Input contract
31/// - valid when `s.len() >= 2 + num_stationary.0 + num_stationary.1`;
32/// - requires `a.len() == s.len()` and `b.len() == s.len()`;
33/// - invalid input returns `(NaN, empty)`.
34///
35/// # Returns
36/// Returns `(t_final, t_s)` where `t_s[i]` is cumulative time at `s[i]`.
37///
38/// # Errors
39/// This function does not return `Result`; invalid inputs are mapped to `(NaN, vec![])`.
40///
41/// # Contract
42/// - `t_s.len() == s.len()` on valid input.
43/// - `t_s[0] == t0` on valid input.
44pub fn s_to_t_topp3(
45    s: &[f64],
46    a: &[f64],
47    b: &[f64],
48    num_stationary: (usize, usize),
49    t0: f64,
50) -> (f64, Vec<f64>) {
51    if s.len() < 2 + num_stationary.0 + num_stationary.1 || a.len() != s.len() || b.len() != s.len()
52    {
53        return (f64::NAN, vec![]);
54    }
55    let mut t_s = Vec::<f64>::with_capacity(s.len()); // t_s[i] = t(s[i]), begin from t0
56    let mut t_prev = t0;
57    let n = s.len() - 1;
58    t_s.push(t_prev);
59    if num_stationary.0 > 0 {
60        let s0 = s.first().unwrap();
61        t_s.resize(1 + num_stationary.0, t_prev);
62        for (t_curr, a_curr, s_curr) in izip!(t_s.iter_mut(), a.iter(), s.iter()).skip(1) {
63            *t_curr += 3.0 * (s_curr - s0) / a_curr.sqrt();
64        }
65        t_prev = *t_s.last().unwrap();
66    }
67    for (s_pair, b_pair, a_curr) in izip!(s.windows(2), b.windows(2), a.iter())
68        .skip(num_stationary.0)
69        .take(n - num_stationary.0 - num_stationary.1)
70    {
71        t_prev += integral_rsrqp(
72            *a_curr,
73            2.0 * b_pair[0],
74            (b_pair[1] - b_pair[0]) / (s_pair[1] - s_pair[0]),
75            0.0,
76            s_pair[1] - s_pair[0],
77        );
78        t_s.push(t_prev);
79    }
80    if num_stationary.1 > 0 {
81        let s_final = s.last().unwrap();
82        let t_final =
83            t_prev + 3.0 * (s_final - s[n - num_stationary.1]) / a[n - num_stationary.1].sqrt();
84        t_s.resize(s.len(), t_final);
85        if num_stationary.1 > 1 {
86            for (t_curr, a_curr, s_curr) in izip!(t_s.iter_mut(), a.iter(), s.iter())
87                .rev()
88                .skip(1)
89                .take(num_stationary.1 - 1)
90            {
91                *t_curr += 3.0 * (s_curr - s_final) / a_curr.sqrt();
92            }
93        }
94    }
95
96    (*t_s.last().unwrap(), t_s)
97}
98
99/// Compute definite integral of reciprocal-square-root quadratic polynomial:
100/// $$dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x + c_2 x^2}}.$$
101fn integral_rsrqp(c0: f64, c1: f64, c2: f64, x_left: f64, x_right: f64) -> f64 {
102    if c2 > f64::EPSILON {
103        let func = |x: f64| x + 0.5 * c1 / c2 + (x * x + (c1 * x + c0) / c2).sqrt();
104        (func(x_right).abs().ln() - func(x_left).abs().ln()) / c2.sqrt()
105    } else if c2 < -f64::EPSILON {
106        let delta = c1 * c1 - 4.0 * c2 * c0;
107        if delta > 0.0 {
108            let func = |x: f64| (-2.0 * c2 * x - c1) / delta.sqrt();
109            (func(x_right).asin() - func(x_left).asin()) / (-c2).sqrt()
110        } else {
111            f64::INFINITY
112        }
113    } else if c1.abs() > f64::EPSILON {
114        // Dt = \int_{xl}^{xr} dx/sqrt(C1*x+C0)
115        2.0 / c1 * ((c1 * x_right + c0).sqrt() - (c1 * x_left + c0).sqrt())
116    } else if c0.abs() > f64::EPSILON {
117        // Dt = \int_{xl}^{xr} dx/sqrt(C0)
118        (x_right - x_left) / c0.sqrt()
119    } else {
120        f64::INFINITY
121    }
122}
123
124/// Interpolate inverse mapping `s(t)` from `a(s)`, `b(s)`, and sampled `t(s)`.
125///
126/// # Modes
127/// - `UniformTimeGrid(t0, dt, include_final)`: generate uniform time samples;
128/// - `NonUniformTimeGrid(t_sample)`: use caller-provided increasing samples.
129///
130/// # Input contract
131/// - requires `s.len() >= 2`, `a.len() == s.len()`, `b.len() == s.len()`, `t_s.len() == s.len()`;
132/// - requires `t_s` strictly increasing;
133/// - invalid input returns empty vector.
134///
135/// # Output semantics
136/// - output length matches requested sample count in each mode;
137/// - for out-of-range time samples, output entries are `NaN`.
138///
139/// # Returns
140/// Returns sampled `s(t)` values under the requested interpolation `mode`.
141///
142/// # Errors
143/// This function does not return `Result`; malformed inputs are mapped to empty output.
144///
145/// # Contract
146/// - preserves caller time-sample ordering.
147/// - never panics on invalid user data paths (returns empty vector).
148pub fn t_to_s_topp3(
149    s: &[f64],
150    a: &[f64],
151    b: &[f64],
152    num_stationary: (usize, usize),
153    t_s: &[f64],
154    mode: InterpolationMode<'_>,
155) -> Vec<f64> {
156    if s.len() < 2
157        || a.len() != s.len()
158        || b.len() != s.len()
159        || t_s.len() != s.len()
160        || t_s.windows(2).any(|w| w[0] >= w[1])
161    {
162        return vec![];
163    }
164    match mode {
165        InterpolationMode::UniformTimeGrid(t0, dt, include_final) => {
166            if dt <= 0.0 {
167                return vec![];
168            }
169            // num_t * dt + t0 <= t_final
170            let num_t = ((t_s.last().unwrap() - t0) / dt).floor() as usize;
171            let mut s_t = t_to_s_topp3_core(
172                s,
173                a,
174                b,
175                num_stationary,
176                t_s,
177                (0..num_t).map(|i| t0 + i as f64 * dt),
178                num_t,
179            );
180            if include_final {
181                let flag = if s_t.is_empty() {
182                    t0 <= *t_s.last().unwrap()
183                } else {
184                    *s_t.last().unwrap() < *s.last().unwrap()
185                };
186                if flag {
187                    s_t.push(*s.last().unwrap());
188                }
189            }
190            s_t
191        }
192        InterpolationMode::NonUniformTimeGrid(t_sample) => {
193            if t_sample.is_empty() || t_sample.windows(2).any(|w| w[0] >= w[1]) {
194                // Exclude the case where t_sample.len() == 1
195                return vec![];
196            }
197            t_to_s_topp3_core(
198                s,
199                a,
200                b,
201                num_stationary,
202                t_s,
203                t_sample.iter().cloned(),
204                t_sample.len(),
205            )
206        }
207    }
208}
209
210fn t_to_s_topp3_core(
211    s: &[f64],
212    a: &[f64],
213    b: &[f64],
214    num_stationary: (usize, usize),
215    t_s: &[f64],
216    mut t_sample: impl Iterator<Item = f64>,
217    len_t_sample: usize,
218) -> Vec<f64> {
219    // Core inverse interpolation kernel for `t_to_s_topp3`.
220    // It consumes increasing `t_sample` values and emits corresponding `s(t)`.
221    // Map t to s
222    let &t_start = t_s.first().unwrap();
223    let &t_final = t_s.last().unwrap();
224    let mut s_t = Vec::<f64>::with_capacity(len_t_sample + 1); // s_t[i] = s(t[i])
225    let Some(mut t_curr) = t_sample.next() else {
226        return vec![];
227    };
228    while t_curr < t_start {
229        s_t.push(f64::NAN);
230        let Some(t) = t_sample.next() else {
231            return s_t;
232        };
233        t_curr = t;
234    }
235
236    if num_stationary.0 > 0 {
237        let s0 = s.first().unwrap();
238        let a_stationary = a[num_stationary.0];
239        let t_stationary = t_s[num_stationary.0];
240        let d3u_over_6 =
241            a_stationary.sqrt() * a_stationary / (27.0 * (s[num_stationary.0] - s0).powi(2));
242        while t_curr <= t_stationary {
243            s_t.push(s0 + d3u_over_6 * (t_curr - t_start).powi(3));
244            let Some(t) = t_sample.next() else {
245                return s_t;
246            };
247            t_curr = t;
248        }
249    }
250
251    for (s_pair, &a_curr, b_pair, t_pair) in
252        izip!(s.windows(2), a.iter(), b.windows(2), t_s.windows(2))
253            .skip(num_stationary.0)
254            .take(s.len() - num_stationary.0 - num_stationary.1 - 1)
255    {
256        while t_curr <= t_pair[1] {
257            s_t.push(
258                s_pair[0]
259                    + inverse_rsrqp(
260                        a_curr,
261                        2.0 * b_pair[0],
262                        (b_pair[1] - b_pair[0]) / (s_pair[1] - s_pair[0]),
263                        0.0,
264                        t_curr - t_pair[0],
265                    ),
266            );
267            let Some(t) = t_sample.next() else {
268                return s_t;
269            };
270            t_curr = t;
271        }
272    }
273
274    if num_stationary.1 > 0 {
275        let s_final = s.last().unwrap();
276        let a_stationary = a[s.len() - num_stationary.1 - 1];
277        let d3u_over_6 = a_stationary.sqrt() * a_stationary
278            / (27.0 * (s_final - s[s.len() - num_stationary.1 - 1]).powi(2));
279        while t_curr <= t_final {
280            s_t.push(s_final + d3u_over_6 * (t_curr - t_final).powi(3));
281            let Some(t) = t_sample.next() else {
282                return s_t;
283            };
284            t_curr = t;
285        }
286    }
287
288    s_t.push(f64::NAN);
289    while t_sample.next().is_some() {
290        s_t.push(f64::NAN);
291    }
292    s_t
293}
294
295/// Solve `x_right` from
296/// $$dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x + c_2 x^2}}.$$
297fn inverse_rsrqp(c0: f64, c1: f64, c2: f64, x_left: f64, dt: f64) -> f64 {
298    if dt == 0.0 {
299        return x_left;
300    }
301    let delta = c1 * c1 - 4.0 * c2 * c0;
302    if c2 > f64::EPSILON {
303        let mu = (c2.sqrt() * dt
304            + (x_left + 0.5 * c1 / c2 + (x_left * x_left + (c1 * x_left + c0) / c2).sqrt())
305                .abs()
306                .ln())
307        .exp();
308        let xr1 = -0.5 * c1 / c2 + 0.5 * (mu + delta / (4.0 * c2 * c2 * mu));
309        let xr2 = -0.5 * c1 / c2 - 0.5 * (mu + delta / (4.0 * c2 * c2 * mu));
310        let mut flag1 = true;
311        let mut flag2 = true;
312        if dt > 0.0 {
313            flag1 &= xr1 > x_left;
314            flag2 &= xr2 > x_left;
315        } else {
316            flag1 &= xr1 < x_left;
317            flag2 &= xr2 < x_left;
318        }
319        if flag1 && flag2 {
320            let dt1 = integral_rsrqp(c0, c1, c2, x_left, xr1);
321            let dt2 = integral_rsrqp(c0, c1, c2, x_left, xr2);
322            if (dt1 - dt).abs() < (dt2 - dt).abs() {
323                xr1
324            } else {
325                xr2
326            }
327        } else if flag1 {
328            xr1
329        } else if flag2 {
330            xr2
331        } else {
332            f64::INFINITY
333        }
334    } else if c2 < -f64::EPSILON {
335        (c1 + delta.sqrt()
336            * ((-c2).sqrt() * dt + ((-2.0 * c2 * x_left - c1) / delta.sqrt()).asin()).sin())
337            / (-2.0 * c2)
338    } else if c1.abs() > f64::EPSILON {
339        ((0.5 * c1 * dt + (c1 * x_left + c0).sqrt()).powi(2) - c0) / c1
340    } else if c0.abs() > f64::EPSILON {
341        c0.sqrt() * dt + x_left
342    } else {
343        f64::INFINITY
344    }
345}
346
347/// Post-process `(a, b)` so that interpolated `a(s)` stays strictly positive per interval.
348///
349/// This is a numerical safety utility for downstream timing integration on
350/// profiles that may be very close to zero due to finite precision.
351///
352/// # Returns
353/// Returns `true` when in-place adjustment succeeds, otherwise `false`.
354///
355/// # Errors
356/// This function does not return `Result`; invalid inputs are reported by `false`
357/// with diagnostic prints.
358///
359/// # Contract
360/// - requires `a.len() == b.len() == s.len()` and `s.len() >= 4`;
361/// - requires endpoint `a` values to be nonnegative.
362pub fn force_positive_a(
363    a: &mut [f64],
364    b: &mut [f64],
365    s: &[f64],
366    num_stationary: (usize, usize),
367    a_min: f64,
368) -> bool {
369    let n = s.len();
370    if a.len() != n || b.len() != n {
371        crate::verbosity_log!(
372            crate::diag::Verbosity::Debug,
373            "force_positive_a: a, b, s should have the same length"
374        );
375        return false;
376    }
377    if n < 4 {
378        crate::verbosity_log!(
379            crate::diag::Verbosity::Debug,
380            "force_positive_a: the length of a, b, s should be at least 4"
381        );
382        return false;
383    }
384    if a.iter().any(|&a| a < 0.0) {
385        crate::verbosity_log!(
386            crate::diag::Verbosity::Debug,
387            "force_positive_a: a should be non-negative at each end point"
388        );
389        return false;
390    }
391    // Now we have a(s[i]) >= 0, and we would like to modify a(s) > 0 for s in (s[i], s[i+1]) if a(s) can be negative for some s in (s[i], s[i+1]).
392    let mut flag_succeed = true;
393    for i in (num_stationary.0 + 1)..(n - 2 - num_stationary.1) {
394        // Consider a[i-1], a[i], a[i+1], a[i+2]
395        let b1 = b[i];
396        let b2 = b[i + 1];
397        if b1 < 0.0 && b2 > 0.0 {
398            // a(s) = a[i] + 2 * b[i] * (s - s[i]) + (b[i+1] - b[i]) / ds1 * (s - s[i])^2
399            // b[i] ^ 2 < a[i] * (b[i+1] - b[i]) / ds1 should hold
400            // b[i] ^ 2 * ds1 < a[i] * (b[i+1] - b[i]) should hold
401            let s1 = s[i];
402            let s2 = s[i + 1];
403            let ds1 = s2 - s1;
404            let a1 = a[i];
405            let amin = a_min.max(a1.min(a[i + 1]));
406            let amin = if amin > 10.0 * EPS_ZERO {
407                0.1 * amin
408            } else if amin > EPS_ZERO {
409                EPS_ZERO
410            } else {
411                amin
412            };
413            let da = a1 - amin;
414            let db = b2 - b1;
415            if b1 * b1 * ds1 >= da * db {
416                // a(s) <= 0 holds in (s[i], s[i+1])
417                // We add c0 on (s[i-1],s[i+2]), c1 on (s[i],s[i+2]), and c2 on (s[i+1],s[i+2])
418                // x[i-1] and x[i+2] should keep the same.
419                // (i) --- c0*(s[i+2] - s[i-1]) + c1*(s[i+2] - s[i]) + c2*(s[i+2] - s[i+1]) == 0
420                // (ii) --- c0*(s[i+2] - s[i-1])^2 + c1*(s[i+2] - s[i])^2 + c2*(s[i+2] - s[i+1])^2 == 0
421                let s0 = s[i - 1];
422                let s3 = s[i + 2];
423                let delta_s_end = (s3 - s0, s3 - s1, s3 - s2);
424                let coeff = match solve_2x2(
425                    (
426                        (delta_s_end.1, delta_s_end.2),
427                        (delta_s_end.1 * delta_s_end.1, delta_s_end.2 * delta_s_end.2),
428                    ),
429                    (-delta_s_end.0, -delta_s_end.0 * delta_s_end.0),
430                ) {
431                    Some(coeff) => {
432                        // A*[c1;c2] = b*c0
433                        coeff
434                    }
435                    None => {
436                        crate::verbosity_log!(
437                            crate::diag::Verbosity::Debug,
438                            "coeff is None? A = {:?}, b = {:?}",
439                            (
440                                (delta_s_end.1, delta_s_end.2),
441                                (delta_s_end.1 * delta_s_end.1, delta_s_end.2 * delta_s_end.2)
442                            ),
443                            (-delta_s_end.0, -delta_s_end.0 * delta_s_end.0)
444                        );
445                        flag_succeed = false;
446                        continue;
447                    }
448                };
449                // c1 = coeff.0 * c0, c2 = coeff.1 * c0
450                // Changes: a[i] += c0 * (s1-s0)^2, b[i] += c0 * (s1-s0), b[i+1] += c0 * (s2-s0) + c1 * (s2-s1)
451                let ds0 = s1 - s0;
452                let coeff_c = (ds0 * ds0, ds0, ds0 + ds1 * (1.0 + coeff.0));
453                // Changes: a[i] += c0 * coeff_c.0, b[i] += c0 * coeff_c.1, b[i+1] += c0 * coeff_c.2
454                // We hope that a(s) = a[i] + 2 * b[i] * (s - s[i]) + (b[i+1] - b[i]) / ds1 * (s - s[i])^2 >= amin holds in (s[i],s[i+1])
455                // b[i] ^ 2 * ds1 == (a[i] - amin) * (b[i+1] - b[i]) should hold for new ones.
456                // For old ones: (b[i] + coeff_c.1 * c0) ^ 2 * ds1 == (a[i] - amin + coeff_c.0 * c0) * (b[i+1] - b[i] + (coeff_c.2-coeff_c.1) * c0). Now solve c0.
457                // (coeff_c.1^2 * c0^2 + 2 * b1 * coeff_c.1 * c0 + b1 ^ 2) * ds1 == coeff_c.0 * (coeff_c.2-coeff_c.1) * c0^2 + (da * (coeff_c.2-coeff_c.1) + coeff_c.0 * db) * c0 + da * db
458                // (coeff_c.1^2 * ds1 - coeff_c.0 * (coeff_c.2-coeff_c.1)) * c0^2 + (2 * b1 * coeff_c.1 * ds1 - da * (coeff_c.2-coeff_c.1) - coeff_c.0 * db) * c0 + (b1 * b1 * ds1 - da * db) == 0
459                let coeff_solve = (
460                    coeff_c.1 * coeff_c.1 * ds1 - coeff_c.0 * (coeff_c.2 - coeff_c.1),
461                    2.0 * b1 * coeff_c.1 * ds1 - da * (coeff_c.2 - coeff_c.1) - coeff_c.0 * db,
462                    b1 * b1 * ds1 - da * db,
463                );
464                let norm = coeff_solve.0.abs() + coeff_solve.1.abs() + coeff_solve.2.abs();
465                if norm < EPS_ZERO {
466                    crate::verbosity_log!(
467                        crate::diag::Verbosity::Debug,
468                        "norm = {norm} < EPS_ZERO, coeff_solve = {coeff_solve:.8?}"
469                    );
470                    flag_succeed = false;
471                    continue;
472                }
473                let norm_inv = 1.0 / norm;
474                let coeff_solve = (
475                    coeff_solve.0 * norm_inv,
476                    coeff_solve.1 * norm_inv,
477                    coeff_solve.2 * norm_inv,
478                );
479                // coeff_solve.0 * c0^2 + coeff_solve.1 * c0 + coeff_solve.2 == 0
480                let c0 = if coeff_solve.0.abs() > EPS_ZERO {
481                    // Use quadratic formula to solve for c0
482                    let discriminant =
483                        coeff_solve.1 * coeff_solve.1 - 4.0 * coeff_solve.0 * coeff_solve.2;
484                    if discriminant < 0.0 {
485                        if coeff_c.1.abs() > EPS_ZERO && coeff_c.2.abs() > EPS_ZERO {
486                            (-b1 / coeff_c.1).min(b2 / coeff_c.2)
487                        } else if coeff_c.1.abs() > EPS_ZERO {
488                            -b1 / coeff_c.1
489                        } else if coeff_c.2.abs() > EPS_ZERO {
490                            b2 / coeff_c.2
491                        } else {
492                            crate::verbosity_log!(
493                                crate::diag::Verbosity::Debug,
494                                "discriminant = {discriminant:.8} < 0 for c0 (i={i}): coeff_solve = {coeff_solve:.8?}, coeff_c = {coeff_c:.8?}"
495                            );
496                            flag_succeed = false;
497                            continue;
498                        }
499                    } else {
500                        let sqrt_discriminant = discriminant.sqrt();
501                        // c0: (max, min)
502                        let c0 = if coeff_solve.0 > 0.0 {
503                            (
504                                (-coeff_solve.1 + sqrt_discriminant) / (2.0 * coeff_solve.0),
505                                (-coeff_solve.1 - sqrt_discriminant) / (2.0 * coeff_solve.0),
506                            )
507                        } else {
508                            (
509                                (-coeff_solve.1 - sqrt_discriminant) / (2.0 * coeff_solve.0),
510                                (-coeff_solve.1 + sqrt_discriminant) / (2.0 * coeff_solve.0),
511                            )
512                        };
513                        if c0.1 >= 0.0 { c0.1 } else { c0.0 }
514                    }
515                } else {
516                    // Linear case
517                    -coeff_solve.2 / coeff_solve.1
518                };
519                a[i] += coeff_c.0 * c0;
520                b[i] += coeff_c.1 * c0;
521                b[i + 1] += coeff_c.2 * c0;
522                a[i + 1] += (coeff_c.0 + (coeff_c.1 + coeff_c.2) * ds1) * c0;
523            }
524        }
525    }
526
527    flag_succeed
528}
copp\copp\copp3/interpolation.rs

copp\copp\copp3/
interpolation.rs
